{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# Rotate Factors"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "This header defines factors that constrain an unknown rotation (`Rot3`) based on how it transforms either full rotations or directions.\n",
        "\n",
        "*   `RotateFactor`: Relates two *incremental* rotations measured in different frames using an unknown rotation relating the frames. If $Z = R \\cdot P \\cdot R^T$, where $P = e^{[p]}$ and $Z = e^{[z]}$ are measured incremental rotations (expressed as angular velocity vectors $p$ and $z$), this factor constrains the unknown rotation $R$ such that $p = R z$. The error is $Rz - p$.\n",
        "*   `RotateDirectionsFactor`: Relates two *directions* (unit vectors, `Unit3`) measured in different frames using an unknown rotation $R$ relating the frames. If $p_{world} = R \\cdot z_{body}$, this factor constrains $R$. The error is the angular difference between the predicted $R z_{body}$ and the measured $p_{world}$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/RotateFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Rot3, Point3, Unit3, Values\n",
        "from gtsam import RotateFactor, RotateDirectionsFactor\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "R = symbol_shorthand.R # For Rotation key"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_header_md"
      },
      "source": [
        "## 1. `RotateFactor`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_desc_md"
      },
      "source": [
        "Constraints an unknown `Rot3` based on corresponding incremental rotation measurements $P$ (predicted/world) and $Z$ (measured/body)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "factor1_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "RotateFactor:   keys = { r0 }\n",
            "isotropic dim=3 sigma=0.001\n",
            "RotateFactor:]\n",
            "p: 0.01 0.02 0.03\n",
            "z:  0.03 -0.01  0.02\n",
            "\n",
            "Error at ground truth R: 700.0\n",
            "Error at noisy R: 699.2869223608442\n"
          ]
        }
      ],
      "source": [
        "# Assume P and Z are small incremental rotations\n",
        "P = Rot3.Expmap(np.array([0.01, 0.02, 0.03]))\n",
        "Z = Rot3.Expmap(np.array([0.03, -0.01, 0.02]))\n",
        "rot_key = R(0)\n",
        "noise1 = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)\n",
        "\n",
        "factor1 = RotateFactor(rot_key, P, Z, noise1)\n",
        "factor1.print(\"RotateFactor: \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "# Ground truth R would satisfy P = R*Z\n",
        "# R = P * Z.inverse()\n",
        "gt_R = P * (Z.inverse())\n",
        "values.insert(rot_key, gt_R)\n",
        "error1_gt = factor1.error(values)\n",
        "print(f\"\\nError at ground truth R: {error1_gt}\")\n",
        "\n",
        "noisy_R = gt_R * Rot3.Expmap(np.array([0.001, -0.001, 0.001]))\n",
        "values.update(rot_key, noisy_R)\n",
        "error1_noisy = factor1.error(values)\n",
        "print(f\"Error at noisy R: {error1_noisy}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_header_md"
      },
      "source": [
        "## 2. `RotateDirectionsFactor`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_desc_md"
      },
      "source": [
        "Constraints an unknown `Rot3` based on corresponding direction measurements $p_{world}$ (predicted/world) and $z_{body}$ (measured/body)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {
        "id": "factor2_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "RotateDirectionsFactor:   keys = { r0 }\n",
            "isotropic dim=2 sigma=0.01\n",
            "RotateDirectionsFactor:\n",
            "p:1\n",
            "0\n",
            "0\n",
            "z:0\n",
            "1\n",
            "0\n",
            "\n",
            "Check: gt_R * z_body = \n",
            ":          1\n",
            "6.12323e-17\n",
            "          0\n",
            "\n",
            "Error at ground truth R: 1.874699728327322e-29\n",
            "Error at noisy R: 0.999933335111092\n"
          ]
        }
      ],
      "source": [
        "p_world = Unit3(Point3(1, 0, 0)) # Direction in world frame\n",
        "z_body = Unit3(Point3(0, 1, 0))  # Corresponding direction in body frame\n",
        "noise2 = gtsam.noiseModel.Isotropic.Sigma(2, 0.01) # Noise on 2D tangent space\n",
        "\n",
        "factor2 = RotateDirectionsFactor(rot_key, p_world, z_body, noise2)\n",
        "factor2.print(\"RotateDirectionsFactor: \")\n",
        "\n",
        "# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)\n",
        "# This corresponds to a -90 deg yaw\n",
        "gt_R_dir = Rot3.Yaw(-np.pi/2)\n",
        "print(f\"\\nCheck: gt_R * z_body = \\n{gt_R_dir.rotate(z_body)}\")\n",
        "\n",
        "# Evaluate error\n",
        "values_dir = Values()\n",
        "values_dir.insert(rot_key, gt_R_dir)\n",
        "error2_gt = factor2.error(values_dir)\n",
        "print(f\"Error at ground truth R: {error2_gt}\")\n",
        "\n",
        "noisy_R_dir = gt_R_dir * Rot3.Expmap(np.array([0.01, 0, 0.01]))\n",
        "values_dir.update(rot_key, noisy_R_dir)\n",
        "error2_noisy = factor2.error(values_dir)\n",
        "print(f\"Error at noisy R: {error2_noisy}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
